{
 "cells": [
  {
   "cell_type": "markdown",
   "source": [
    "## Позиции звеньев манипулятора и результирующая матрица поворота\n",
    "\n",
    "![screenshot](../static/lab4.jpg)"
   ],
   "metadata": {
    "collapsed": false
   }
  },
  {
   "cell_type": "markdown",
   "source": [
    "#### Импорты"
   ],
   "metadata": {
    "collapsed": false
   }
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "import math\n",
    "\n",
    "import numpy as np\n",
    "import sympy as sy\n",
    "\n",
    "from lab4.function4lab import positionsAndRotate"
   ]
  },
  {
   "cell_type": "markdown",
   "source": [
    "#### Координаты звеньев (theta1, d2, theta3)"
   ],
   "metadata": {
    "collapsed": false
   }
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "outputs": [],
   "source": [
    "joint_coords = [math.pi, -10, math.pi / 2]"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "markdown",
   "source": [
    "#### Параметры Денавита-Хартенберга манипулятора"
   ],
   "metadata": {
    "collapsed": false
   }
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "outputs": [],
   "source": [
    "theta1, theta3, d2 = sy.symbols('theta1'), sy.symbols('theta3'), sy.symbols('d2')\n",
    "\n",
    "d = [10, d2, 0, 5]\n",
    "r = [0, 0, 0, 0]\n",
    "thetas = [theta1, -math.pi / 2, -math.pi / 2 - math.pi / 4 + theta3, -math.pi / 2]\n",
    "alpha = [-math.pi / 2 - math.pi / 4, -math.pi / 2, -math.pi / 2, 0]\n",
    "\n",
    "arg = {theta1: joint_coords[0], d2: joint_coords[1], theta3: joint_coords[2]}"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "markdown",
   "source": [
    "#### Вычислении позиций и матрицы поворота"
   ],
   "metadata": {
    "collapsed": false
   }
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "outputs": [],
   "source": [
    "pos, R = positionsAndRotate(d, r, thetas, alpha, arg)"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "markdown",
   "source": [
    "##### Позиции"
   ],
   "metadata": {
    "collapsed": false
   }
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[ 0.     0.     0.   ]\n",
      " [ 0.     0.    10.   ]\n",
      " [ 0.     7.071 17.071]\n",
      " [ 0.     7.071 17.071]\n",
      " [ 0.     7.071 22.071]]\n"
     ]
    }
   ],
   "source": [
    "print(np.round(pos, 3))"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "markdown",
   "source": [
    "##### Матрица поворота"
   ],
   "metadata": {
    "collapsed": false
   }
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[-1. -0. -0.]\n",
      " [ 0. -1.  0.]\n",
      " [-0.  0.  1.]]\n"
     ]
    }
   ],
   "source": [
    "print(np.round(R, 2))"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 0
}